﻿using System;
using System.Diagnostics;

namespace GameStructure.Types
{
    public class Matrix
    {
        private double _m11, _m12, _m13;
        private double _m21, _m22, _m23;
        private double _m31, _m32, _m33;
        private double _m41, _m42, _m43;

        public static readonly Matrix Identity = new Matrix(
            new Vector(1, 0, 0), 
            new Vector(0, 1, 0), 
            new Vector(0, 0, 1), 
            new Vector(0, 0, 1));

        public Matrix() : this(Identity)
        { }

        public Matrix(Matrix m)
        {
            _m11 = m._m11; _m12 = m._m12; _m13 = m._m13;
            _m21 = m._m21; _m22 = m._m22; _m23 = m._m23;
            _m31 = m._m31; _m32 = m._m32; _m33 = m._m33;
            _m41 = m._m41; _m42 = m._m42; _m43 = m._m43;
        }

        public Matrix(Vector x, Vector y, Vector z, Vector o)
        {
            _m11 = x.X; _m12 = x.Y; _m13 = x.Z;
            _m21 = y.X; _m22 = y.Y; _m23 = y.Z;
            _m31 = z.X; _m32 = z.Y; _m33 = z.Z;
            _m41 = o.X; _m42 = o.Y; _m43 = o.Z;
        }

        public void SetTranslation(Vector translation)
        {
            _m41 = translation.X;
            _m42 = translation.Y;
            _m43 = translation.Z;
        }

        public Vector GetTranslation()
        {
            return new Vector(_m41, _m42, _m43);
        }

        public void SetScale(Vector scale)
        {
            _m11 = scale.X;
            _m22 = scale.Y;
            _m33 = scale.Z;
        }

        public Vector GetScale()
        {
            var result = new Vector
            {
                X = (new Vector(_m11, _m12, _m13)).Length(),
                Y = (new Vector(_m21, _m22, _m23)).Length(),
                Z = (new Vector(_m31, _m32, _m33)).Length()
            };
            return result;
        }

        public void SetRotate(Vector axis, double angle)
        {
            var angleSin = Math.Sin(angle);
            var angleCos = Math.Cos(angle);
            var a = 1.0 - angleCos;
            var ax = a*axis.X;
            var ay = a*axis.Y;
            var az = a*axis.Z;

            _m11 = ax*axis.X + angleCos;
            _m12 = ax*axis.Y + axis.Z*angleSin;
            _m13 = ax*axis.Z - axis.Y*angleSin;

            _m21 = ay*axis.X - axis.Z*angleSin;
            _m22 = ay*axis.Y + angleCos;
            _m23 = ay*axis.Z + axis.X*angleSin;

            _m31 = az*axis.X + axis.Y*angleSin;
            _m32 = az*axis.Y - axis.X*angleSin;
            _m33 = az*axis.Z + angleCos;
        }

        public double Determinate()
        {
            return _m11*(_m22*_m33 - _m23*_m32) +
                   _m12*(_m23*_m31 - _m21*_m33) +
                   _m13*(_m21*_m32 - _m22*_m31);
        }

        public Matrix Inverse()
        {
            var determinate = Determinate();
            Debug.Assert(Math.Abs(determinate) > double.Epsilon, "No determinate");

            var oneOverDet = 1.0/determinate;
            var result = new Matrix
            {
                _m11 = (_m22*_m33 - _m23*_m32)*oneOverDet,
                _m12 = (_m13*_m32 - _m12*_m33)*oneOverDet,
                _m13 = (_m12*_m23 - _m13*_m22)*oneOverDet,

                _m21 = (_m23*_m31 - _m21*_m33)*oneOverDet,
                _m22 = (_m11*_m33 - _m13*_m31)*oneOverDet,
                _m23 = (_m13*_m21 - _m11*_m23)*oneOverDet,

                _m31 = (_m21*_m32 - _m22*_m31)*oneOverDet,
                _m32 = (_m12*_m31 - _m11*_m32)*oneOverDet,
                _m33 = (_m11*_m22 - _m12*_m21)*oneOverDet
            };

            result._m41 = -(_m41*result._m11 + _m42*result._m21 + _m43*result._m31);
            result._m42 = -(_m41*result._m12 + _m42*result._m22 + _m43*result._m32);
            result._m43 = -(_m41*result._m13 + _m42*result._m23 + _m43*result._m33);

            return result;
        }

        public static Matrix operator *(Matrix mA, Matrix mB)
        {
            var result = new Matrix
            {
                _m11 = mA._m11*mB._m11 + mA._m12*mB._m21 + mA._m13*mB._m31,
                _m12 = mA._m11*mB._m12 + mA._m12*mB._m22 + mA._m13*mB._m32,
                _m13 = mA._m11*mB._m13 + mA._m12*mB._m23 + mA._m13*mB._m33,

                _m21 = mA._m21*mB._m11 + mA._m22*mB._m21 + mA._m23*mB._m31,
                _m22 = mA._m21*mB._m12 + mA._m22*mB._m22 + mA._m23*mB._m32,
                _m23 = mA._m21*mB._m13 + mA._m22*mB._m23 + mA._m23*mB._m33,

                _m31 = mA._m31*mB._m11 + mA._m32*mB._m21 + mA._m33*mB._m31,
                _m32 = mA._m31*mB._m12 + mA._m32*mB._m22 + mA._m33*mB._m32,
                _m33 = mA._m31*mB._m13 + mA._m32*mB._m23 + mA._m33*mB._m33,

                _m41 = mA._m41*mB._m11 + mA._m42*mB._m21 + mA._m43*mA._m31 + mB._m41,
                _m42 = mA._m41*mB._m12 + mA._m42*mB._m22 + mA._m43*mB._m32 + mB._m42,
                _m43 = mA._m41*mB._m13 + mA._m42*mB._m23 + mA._m43*mB._m33 + mB._m43
            };

            return result;
        }

        public static Vector operator *(Vector v, Matrix m)
        {
            return new Vector(v.X*m._m11 + v.Y*m._m21 + v.Z*m._m31 + m._m41,
                v.X*m._m12 + v.Y*m._m22 + v.Z*m._m32 + m._m42,
                v.X*m._m13 + v.Y*m._m23 + v.Z*m._m33 + m._m43);
        }
    }
}